#include "common/common.h"
#include "motor.h"
#include "external.h"

#include <math.h>

#define WHEEL_RADIUS (0.0625)
#define FACTOR_30K   (0.00825)
#define WHEEL_SPAN   (0.368)
#ifndef M_PIf
#define M_PIf  3.14159265358979f
#endif

RecvData recvData;
static Speed        speed;
static Transfer * transfer;


static long map(long x, long in_min, long in_max, long out_min, long out_max)
{
  return (x - in_min) * (out_max - out_min) / (in_max - in_min) + out_min;
}
/*add by yujikang*/
void transfer_init(USART_TypeDef* _USARTx)
{
  // memset(&motorEncoder, 0, sizeof(MotorEncoder));
  memset(&recvData, 0, sizeof(RecvData));
  memset(&speed, 0, sizeof(Speed));
  //recvData.sensor_dis.size = CONFIG_IR_CHANNEL_NUM;
  transfer = begin(details(recvData), details(speed), _USARTx);
}

void get_external_data()
{
//  // DBG_OUT("sizeof MotorEncoder =  %d\r\n", sizeof(MotorEncoder));
//  // DBG_OUT("sizeof IrBeacon =  %d\r\n", sizeof(IrBeacon));
//  // DBG_OUT("sizeof SensorDistance =  %d\r\n", sizeof(SensorDistance));
//  // DBG_OUT("sizeof RecvData =  %d\r\n", sizeof(RecvData));
//  if (receiveData(transfer)) {
//// #if defined(CONFIG_BREAKOUT_REV) && (CONFIG_BREAKOUT_REV == 3)
////     DBG_OUT("sensor_dis0 =  %d\r\n", recvData.sensor_dis.distance[0]);
////     DBG_OUT("sensor_dis1 =  %d\r\n", recvData.sensor_dis.distance[1]);
////     DBG_OUT("sensor_dis2 =  %d\r\n", recvData.sensor_dis.distance[2]);
////     DBG_OUT("sensor_dis3 =  %d\r\n", recvData.sensor_dis.distance[3]);
////     // DBG_OUT("sensor_dis =  %d\r\n", recvData.motorEncoder.ticksDeltaR);
//// #endif      
//  	addTicks(recvData.motorEncoder.ticksDeltaL, recvData.motorEncoder.ticksDeltaR);
//    // todo set sharp function here
//  }
		/*  Edit by LAY  start */
		static uint32_t MotorBiasDataHeartbeatFrequency;
    if ((getms() - MotorBiasDataHeartbeatFrequency) >= CONFIG_MOTOR_BIAS_DATA_TIMEOUT_MS) {
			MotorBiasDataHeartbeatFrequency = getms();
			if (send_Motor_Query(transfer)) {
					if(receiveData(transfer))
					{
						recvData.motorEncoder.ticksDeltaL = g_iMoto1BiasData;
						recvData.motorEncoder.ticksDeltaR = g_iMoto2BiasData;			
  					addTicks(recvData.motorEncoder.ticksDeltaL, recvData.motorEncoder.ticksDeltaR);
					}
	 		}
		}
		/*  Edit by LAY  stop */
}




static double rpmToValue(double rpm)
{
  return rpm / FACTOR_30K;
}
/*
 * lin 速度 m / s
 * lin * 60 :一分钟走的路程
 * 
*/
static double linearToRpm(double lin)
{
  double mileage = lin * 60.0;//mileage in one minute
  double rpm = mileage / (2.0 * M_PIf * WHEEL_RADIUS) / 1000.0;// RPM = mileage / (2*pi*radius)
  
  return rpm;
}

// _s32 calcRad(const _s32 lspeed, const _s32 rspeed)
// {
//   return WHEEL_SPAN * 1000.0 / 2.0 * ((double)(lspeed + rspeed)) / ((double)(rspeed - lspeed));
// }


// _s32 calcSpeed(const _s32 lspeed, const _s32 rspeed)
// {
//   return (_s32)((double)(rspeed - lspeed) * WHEEL_SPAN * 1000.0 * calcSpeed(lspeed, rspeed));
// }

void send_external_data(const _s32 lspeed, const _s32 rspeed)
{
  double left_speed  = (double)lspeed;
  double right_speed = (double)rspeed;
  double multi       = 1.0;

  if ((lspeed > 0 && rspeed > 0) || (lspeed < 0 && rspeed < 0)) {
    _u32 delta = abs(lspeed - rspeed);
    if (delta < 20) {
      multi = 1.6;
    } else if (delta < 40 && delta >= 20) {
      multi = 1.5;
    } else if (delta < 60 && delta >= 40) {
      multi = 1.4;
    } else if (delta < 80 && delta >= 60) {
      multi = 1.3;
    } else if (delta < 100 && delta >= 80) {
      multi = 1.2;
    } else {
      multi = 1.1;
    }
  }

  left_speed  *= multi;
  right_speed *= multi;

  speed.left_wheel = (_s32)rpmToValue(linearToRpm(left_speed));
  speed.right_wheel = (_s32)rpmToValue(linearToRpm(right_speed));//_motorSpeedMm[WALKINGMOTOR_RIGHT_ID];
  speed.left_wheel = -(speed.left_wheel);//2个电机是反向安装的，所以有一个得取反
	

// #if defined(CONFIG_BREAKOUT_REV) && (CONFIG_BREAKOUT_REV == 3)
//     if (speed.left_wheel != 0 || speed.right_wheel != 0) {
//       DBG_OUT("speed.left_wheel =  %d\r\n", speed.left_wheel);
//       DBG_OUT("speed.right_wheel =  %d\r\n", speed.right_wheel); 
// 		}
// #endif
		
	/*  Edit by LAY  start */
	//sendData(transfer);
	sendMotorData(transfer,speed.left_wheel,speed.right_wheel);	
	/*  Edit by LAY  stop */
}

/*add by yujikang*/

/**临时放在这里*/
uint8_t ex_homeir_getmaindata(void) 
{
    // DBG_OUT("beacon = %x\r\n", recvData._irBeacon.beacon);
    return (recvData._irBeacon.beacon >> 4) & 0x0f;
}

/**临时放在这里*/
uint8_t ex_homeir_getleftdata(void)
{
    return (recvData._irBeacon.beacon >> 8) & 0x0f;
}
/**临时放在这里*/
uint8_t ex_homeir_getrightdata(void)
{
    return (recvData._irBeacon.beacon) & 0x0f;
}
/**临时放在这里*/

uint32_t ex_sharpir_get(int ch)
{
    // DBG_OUT("recvData.sensor_dis.distance[ch] = %d\r\n", recvData.sensor_dis.distance[ch]);
    int distanceCM = 27.728 * pow(map(recvData.sensor_dis.distance[ch], 0, 1023, 0, 5000)/1000.0, -1.2045);
    distanceCM = distanceCM < 80 ? distanceCM : 1000;
    return (uint32_t)(distanceCM * 10);
}